"""
# pytest tests/EPA_test.py
物理碰撞算法测试

- 碰撞检测: GJK算法
- 穿透向量计算: EPA算法
"""
import numpy as np
from ShapePhysics import ShapeBase, BoxCheck, GJKCheck, Simplex, Edge, EPAPolygon, to_np


def test_norm():
    """
    计算穿透向量的膜
    """
    a, b = [0, 2], [2, 0]
    e = Edge(a, b)
    e.normal
    e.distance

    # 穿透向量校验 e.normal*distance == [1, 1]
    assert (np.round(e.normal * e.distance - np.array([1, 1]), 10) == 0).all(), '穿透向量e应等于[1, 1], |e|应为sqrt(2)!'
    return 1


def test_EPA_core(aa, bb, correct_EPA_answer=None):
    """
    EPA在碰撞的情况
    """
    aa: ShapeBase = ShapeBase(aa)
    bb: ShapeBase = ShapeBase(bb)

    simp = Simplex()  # 记录包含原点的单纯形, 用EPA来反推碰撞向量
    is_collision = GJKCheck(aa, bb, simp)
    print('--- is_collision: ', is_collision)

    assert is_collision is True, 'is_collision没有碰撞!'

    from ShapePhysics import EPA_Algorithom
    ret = EPA_Algorithom(aa, bb, simp)
    print('--------- compare ---------')
    print('~~~~~~ ret: ', ret)
    print('--- is_collision: ', is_collision, ', ------ simp:', simp)
    return ret


def test_EPA_1():
    a_0 = [
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ]
    b_0 = [
        [0, 0],
        [3, 0],
        [3, 3],
        [0, 3],
    ]
    correct_EPA_answer = [1, 1]
    test_EPA_core(a_0, b_0, correct_EPA_answer)


def test_EPA_2():
    a_0 = -2 * np.array([
        [1, 1],
        [-1, 1],
        [-1, -1],
        [1, -1],
    ])
    # b_0 = [
    #     [1, 0],
    #     [3, 0],
    #     [3, 3],
    #     # [0, 3],
    # ]
    b_0 = - np.array([
        [1.7, 0],
        [3, 3],
        [3, -3],
    ])

    a_0.shape[0] * b_0.shape[0]
    test_EPA_core(a_0, b_0)


# def test_EPAClass():
#     if 1:
#         aa = 2 * np.array([
#             [1, 1],
#             [-1, 1],
#             [-1, -1],
#             [1, -1],
#         ])
#
#         bb = np.array([
#             [1.9, 0],
#             # [2, -3],
#             [3, 3],
#             [3, -3],
#         ])
#
#         simp = Simplex()  # 记录包含原点的单纯形
#         is_collision = _test_core(aa, bb, simp)
#         assert is_collision is True, 'Return Value Error!'
#
#     from EPA import EPAClass
#     epa_class = EPAClass(simp)
#     print('111')

    pass


if __name__ == '__main__':
    test_EPA_2()
    # test_EPAClass()
    1